/**
 * 
 * @File    : 
 * @Brief   : ... 
 * @Version : 1.0
 * @Author  : 靳普诏
 * @E-Mail  : 499367619@qq.com
 */
/**
 * @file        rte_bot_car_place.c
 * @brief       XXXX
 * @note        XXXX
 * @author      靳普诏(puzhao.jin@hopechart.com)
 * @date        2024/04/21
 * @version     1.0

 * @par         修改日志
 * <table>
 * <tr><th>Date         <th>Version     <th>Author      <th> Description
 * <tr><td>2024/04/21   <td>1.0         <td>靳普诏       <td> 创建初始版本
 * @copyright 杭州鸿泉物联网技术股份有限公司
 */
 
#include "rte_bot_car_place.h"
#include <string.h>

enum
{
    kRBCarPlaceStateInit = 0,
    kRBCarPlaceStateValid = 1,
    kRBCarPlaceStateWait = 2,

    kRBCarPlaceOffsetXNone = 0,
    kRBCarPlaceOffsetXFront = 1,
    kRBCarPlaceOffsetXRear = 2,

    kRBCarPlaceOffsetYNone = 0,
    kRBCarPlaceOffsetYFront = 1,
    kRBCarPlaceOffsetYRear = 2,
};

/* ------------------------------------------------------------------ */
static Int32 L_Rte_TBotCarPlaceLaserValXF(Rte_TBotCarPlace *self)
{
    Int8 idx = self->laser_xf_idx;
    Int32 result = self->laser_orig_data_[idx];

    if (self->state_ != kRBCarPlaceStateInit)
        result = result + self->laser_offset_[idx];

    return result;
}

static Int32 L_Rte_TBotCarPlaceLaserValXB(Rte_TBotCarPlace *self)
{
    Int8 idx = self->laser_xb_idx;
    Int32 result = self->laser_orig_data_[idx];

    if (self->state_ != kRBCarPlaceStateInit)
        result = result + self->laser_offset_[idx];

    return result;
}

static Int32 L_Rte_TBotCarPlaceLaserValYF(Rte_TBotCarPlace *self)
{
    Int8 idx = self->laser_yf_idx;
    Int32 result = self->laser_orig_data_[idx];

    if (self->state_ != kRBCarPlaceStateInit)
        result = result + self->laser_offset_[idx];

    return result;
}

static Int32 L_Rte_TBotCarPlaceLaserValYB(Rte_TBotCarPlace *self)
{
    Int8 idx = self->laser_yb_idx;
    Int32 result = self->laser_orig_data_[idx];

    if (self->state_ != kRBCarPlaceStateInit)
        result = result + self->laser_offset_[idx];

    return result;
}

static Int32 L_Rte_TbotCarPlaceLaserOffsetXF(Rte_TBotCarPlace *self)
{
    Int32 x_val = L_Rte_TBotCarPlaceLaserValXF(self);

    if (self->flag_x_offset == kRBCarPlaceOffsetXFront)
    {
        x_val = x_val + CAB_X_OFFSEET_LONG;
    }   
    else if (self->flag_x_offset == kRBCarPlaceOffsetXRear)
    {
        x_val = x_val + CAB_X_OFFSEET_SHORT;
    }
                      
    return x_val;
}

static Int32 L_Rte_TbotCarPlaceLaserOffsetXB(Rte_TBotCarPlace *self)
{
    Int32 x_val = L_Rte_TBotCarPlaceLaserValXB(self);
    
    if (self->state_ == kRBCarPlaceStateInit)
        ;
    else if (self->flag_x_offset == kRBCarPlaceOffsetXFront)
    {
        x_val = x_val + CAB_X_OFFSEET_SHORT;
    }   
    else if (self->flag_x_offset == kRBCarPlaceOffsetXRear)
    {
        x_val = x_val + CAB_X_OFFSEET_LONG;
    }
                      
    return x_val;
}

static Int32 L_Rte_TbotCarPlaceLaserOffsetYF(Rte_TBotCarPlace *self)
{
    Int32 y_val = L_Rte_TBotCarPlaceLaserValYF(self);
    
    if (self->state_ == kRBCarPlaceStateInit)
        ;
    else if (self->flag_y_offset == kRBCarPlaceOffsetYFront)
    {
        y_val = y_val + CAB_Y_OFFSEET_LONG;
    }   
    else if (self->flag_y_offset == kRBCarPlaceOffsetYRear)
    {
        y_val = y_val + CAB_Y_OFFSEET_SHORT;
    }

    return y_val;
}

static Int32 L_Rte_TbotCarPlaceLaserOffsetYB(Rte_TBotCarPlace *self)
{
    Int32 y_val = L_Rte_TBotCarPlaceLaserValYB(self);
    
    if (self->state_ == kRBCarPlaceStateInit)
        ;
    else if (self->flag_y_offset == kRBCarPlaceOffsetYFront)
    {
        y_val = y_val + CAB_Y_OFFSEET_SHORT;
    }   
    else if (self->flag_y_offset == kRBCarPlaceOffsetYRear)
    {
        y_val = y_val + CAB_Y_OFFSEET_LONG;
    }

    return y_val;
}
/* ------------------------------------------------------------------ */

// 原始数据X有效
static Bool L_Rte_TBotCarPlaceOrigXIsFull(Rte_TBotCarPlace *self)
{
    Bool result = False;

    if (self->state_ != kRBCarPlaceStateInit)
    {
        Int32 x_val = L_Rte_TBotCarPlaceLaserValXF(self) 
                      + L_Rte_TBotCarPlaceLaserValXB(self);
                      
        if (x_val >= PLAT_X_MAX / 3)
            result = True;
    }

    return result;
}

// 原始数据Y有效
static Bool L_Rte_TBotCarPlaceOrigYIsFull(Rte_TBotCarPlace *self)
{
    Bool result = False;
    
    if (self->state_ != kRBCarPlaceStateInit)
    {
        Int32 y_val = L_Rte_TBotCarPlaceLaserValYF(self) 
                      + L_Rte_TBotCarPlaceLaserValYB(self);
                      
        if (y_val >= PLAT_Y_MAX / 3)    
            result = True;
    }
    
    return result;
}

static Int32 L_Rte_TBotCarPlaceXPos(Rte_TBotCarPlace *self)
{
    Int32 x_pos = 0;

    if (L_Rte_TBotCarPlaceLaserValXF(self) <= L_Rte_TBotCarPlaceLaserValXB(self))
    {
        x_pos = PLAT_X_MAX - L_Rte_TbotCarPlaceLaserOffsetXF(self);
    }
    else
    {
        x_pos = L_Rte_TbotCarPlaceLaserOffsetXB(self);
    }

    return x_pos;
}

static Int32 L_Rte_TBotCarPlaceYPos(Rte_TBotCarPlace *self)
{
    Int32 y_pos = 0;

    if (L_Rte_TBotCarPlaceLaserValYF(self) <= L_Rte_TBotCarPlaceLaserValYB(self))
    {
        y_pos = PLAT_Y_MAX - L_Rte_TbotCarPlaceLaserOffsetXF(self);
    }
    else
    {
        y_pos = L_Rte_TbotCarPlaceLaserOffsetXB(self);
    }

    return y_pos;
}


static Int32 L_Rte_TBotCarPlaceFlagXOffsetCleck(Rte_TBotCarPlace *self)
{
    Int32 result = kRBCarPlaceOffsetXNone;

    if (self->flag_x_offset == kRBCarPlaceOffsetXFront)
    {
        if (L_Rte_TBotCarPlaceOrigXIsFull(self))
        {
            result = kRBCarPlaceOffsetXNone;
        }
    }
    else if (self->flag_x_offset == kRBCarPlaceOffsetXRear)
    {
        if (L_Rte_TBotCarPlaceOrigXIsFull(self))
        {
            result = kRBCarPlaceOffsetXNone;
        }
    }
    else if (self->flag_x_offset == kRBCarPlaceOffsetXNone)
    {
        if (!L_Rte_TBotCarPlaceOrigXIsFull(self)) 
        {
            if (self->place_calc_x_ < (PLAT_X_MAX / 2))
            {
                result = kRBCarPlaceOffsetXFront;
            }
            else if (self->place_calc_x_ > (PLAT_X_MAX / 2))
            {
                result = kRBCarPlaceOffsetXRear;
            }
        }
    }
    
    return result;
}

static Int32 L_Rte_TBotCarPlaceFlagYOffsetCleck(Rte_TBotCarPlace *self)
{
    Int32 result = kRBCarPlaceOffsetYNone;

    if (self->flag_y_offset == kRBCarPlaceOffsetYFront)
    {
        if (L_Rte_TBotCarPlaceOrigYIsFull(self))
        {
            result = kRBCarPlaceOffsetYNone;
        }
    }
    else if (self->flag_y_offset == kRBCarPlaceOffsetYRear)
    {
        if (L_Rte_TBotCarPlaceOrigYIsFull(self))
        {
            result = kRBCarPlaceOffsetYNone;
        }
    }
    else if (self->flag_y_offset == kRBCarPlaceOffsetYNone)
    {
        if (!L_Rte_TBotCarPlaceOrigYIsFull(self)) 
        {
            if (self->place_calc_y_ < (PLAT_Y_MAX / 2))
            {
                result = kRBCarPlaceOffsetYFront;
            }
            else if (self->place_calc_y_ > (PLAT_Y_MAX / 2))
            {
                result = kRBCarPlaceOffsetYRear;
            }
        }
    }
    
    return result;
}

/* ------------------------------------------------------------------ */

static void L_Rte_TBotCarPlaceStateChange(Rte_TBotCarPlace *self, Int32 next_state)
{
    self->state_ = next_state;
    self->updata_cnt_ = 0;

    if (next_state == kRBCarPlaceStateInit)
    {
        self->laser_offset_[0] = 0;
        self->laser_offset_[1] = 0;
        self->laser_offset_[2] = 0;
        self->laser_offset_[3] = 0;
    }
}


static void L_Rte_TBotCarPlaceUpdataOnInit(Rte_TBotCarPlace *self)
{
    // 校准激光
    Int32 xf = L_Rte_TBotCarPlaceLaserValXF(self);
    Int32 xb = L_Rte_TBotCarPlaceLaserValXB(self);
    Int32 yf = L_Rte_TBotCarPlaceLaserValYF(self);
    Int32 yb = L_Rte_TBotCarPlaceLaserValYB(self);

    Int32 plat_x_limit = PLAT_X_MAX;
    Int32 plat_y_limit = PLAT_Y_MAX;

    if (self->flag_y_offset == kRBCarPlaceOffsetXFront)
    {
        plat_y_limit = PLAT_Y_MAX - CAB_Y_OFFSEET_LONG;
    }

    self->laser_offset_[self->laser_xf_idx] = plat_x_limit - ((xf + xb) / 2) - xf;
    self->laser_offset_[self->laser_xb_idx] = ((xf + xb) / 2) - xb;
    self->laser_offset_[self->laser_yf_idx] = plat_y_limit - ((yf + yb) / 2) - yf;
    self->laser_offset_[self->laser_yb_idx] = ((yf + yb) / 2) - yb;

    // 循环执行10次
    if (self->updata_cnt_ > 10)
    {
        L_Rte_TBotCarPlaceStateChange(self, kRBCarPlaceStateValid);
    }
}

static void L_Rte_TBotCarPlaceUpdataOnValid(Rte_TBotCarPlace *self)
{
    Int32 curr_x = L_Rte_TBotCarPlaceXPos(self);
    Int32 curr_y = L_Rte_TBotCarPlaceYPos(self);
    Bool curr_state_need_change = False;

    if (L_Rte_TBotCarPlaceFlagXOffsetCleck(self) != self->flag_x_offset
        || L_Rte_TBotCarPlaceFlagYOffsetCleck(self) != self->flag_y_offset)
    {
        curr_state_need_change = True;
    }

    // 计算位置
    if (curr_state_need_change)
    {
        L_Rte_TBotCarPlaceStateChange(self, kRBCarPlaceStateWait);
    }
    else
    {
        self->place_calc_x_ = curr_x;
        self->place_calc_y_ = curr_y;
    }
}

static void L_Rte_TBotCarPlaceUpdataOnWait(Rte_TBotCarPlace *self)
{
    self->flag_x_offset = L_Rte_TBotCarPlaceFlagXOffsetCleck(self);
    self->flag_y_offset = L_Rte_TBotCarPlaceFlagYOffsetCleck(self);
    L_Rte_TBotCarPlaceStateChange(self, kRBCarPlaceStateValid);
}

/* ------------------------------------------------------------------ */

Int32 Rte_TBotCarPlaceCreate(Rte_TBotCarPlace *self)
{
    if (self == NULL)
        return -1;
    
    memset(self, 0, sizeof(*self));
    return 0;
}

Bool Rte_TBotCarPlaceInit(Rte_TBotCarPlace *self)
{
    self->flag_x_offset = kRBCarPlaceOffsetXNone;
    self->flag_y_offset = kRBCarPlaceOffsetYFront;

    self->laser_xf_idx = 0;
    self->laser_xb_idx = 1;
    self->laser_yf_idx = 2;
    self->laser_yb_idx = 3;

    L_Rte_TBotCarPlaceStateChange(self, kRBCarPlaceStateInit);

    return True;
}

Bool Rte_TBotCarPlaceUpdata(Rte_TBotCarPlace *self, Int32 orig_data[4])
{
    self->laser_orig_data_[0] = orig_data[0];
    self->laser_orig_data_[1] = orig_data[1];
    self->laser_orig_data_[2] = orig_data[2];
    self->laser_orig_data_[3] = orig_data[3];

    switch (self->state_)
    {
        case kRBCarPlaceStateInit:
        {
            L_Rte_TBotCarPlaceUpdataOnInit(self);
            break;
        }
        case kRBCarPlaceStateValid:
        {
            L_Rte_TBotCarPlaceUpdataOnValid(self);
            break;
        }
        case kRBCarPlaceStateWait:
        {
            L_Rte_TBotCarPlaceUpdataOnWait(self);
            break;
        }
        default:
            break;
    } 

    self->updata_cnt_++;

    return True;
}

Bool Rte_TBotCarPlaceGetPlace(Rte_TBotCarPlace *self, Int32 *x, Int32 *y)
{
    if (x != NULL && y != NULL)
    {
        *x = self->place_calc_x_;
        *y = self->place_calc_y_;
    }

    return self->state_ == kRBCarPlaceStateValid;
}

void Rte_TBotCarPlaceDone(Rte_TBotCarPlace *self)
{

}

void Rte_TBotCarPlaceDestroy(Rte_TBotCarPlace *self)
{
    
}





///< Generated on "2024-04-21 12:00:44" by the tool "gen_hq_file.py >> V20231119" 

